#include <Arduino.h>
#include "PS2X_lib.h"     // PS2手柄控制库
#include <ESP32Servo.h>   // ESP32舵机控制库

/*------------------------ 硬件引脚配置 ------------------------*/

#define PS2_DAT  19       // 手柄MISO引脚
#define PS2_CMD  23       // 手柄MOSI引脚
#define PS2_SEL   5       // 手柄片选引脚
#define PS2_CLK  18       // 手柄时钟引脚
#define IN1  13           // 电机驱动H桥输入1
#define IN2  12           // 电机驱动H桥输入2
#define ENA  25           // 电机使能/PWM控制
#define pressures   false // 禁用压力感应
#define rumble      false // 禁用震动功能
#define motor 32          // 备用电机引脚（当前未使用）

/*------------------------ 设备对象初始化 -----------------------*/

PS2X ps2x;                // PS2手柄控制器实例
Servo myServo;            // 舵机控制器实例
int servoPin = 17;        // 舵机信号线连接引脚
int servoAngle = 90;      // 舵机初始角度（居中位置）

/*------------------------ 系统状态变量 ------------------------*/

int error = -1;           // 手柄连接错误状态（-1表示未初始化）
byte type = 0;            // 检测到的手柄类型
byte vibrate = 0;         // 震动功能强度（当前未使用）
int tryNum = 1;           // 手柄连接尝试次数计数器
int a1,a2;                // 临时计算变量（a1用于舵机角度增量）
int v=0;                  // 电机PWM速度值（0-255）

/*======================= 初始化函数 ========================*/
void setup() {
  Serial.begin(115200);   // 初始化串口通信
  
  // 舵机初始化
  myServo.attach(servoPin);    // 绑定舵机到指定引脚
  myServo.write(servoAngle);   // 设置初始角度

  // 电机驱动引脚初始化
  pinMode(motor, OUTPUT);      // 备用电机引脚（未启用）
  pinMode(IN1, OUTPUT);        // H桥输入1设为输出
  pinMode(IN2, OUTPUT);        // H桥输入2设为输出
  pinMode(ENA, OUTPUT);        // 使能/PWM引脚设为输出

  /*---------- PS2手柄连接初始化 ----------*/
  while (error != 0) {         // 持续尝试直到连接成功
    delay(1000);               // 每次尝试间隔1秒
    // 配置手柄引脚参数：时钟、命令、片选、数据、压力、震动
    error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
    Serial.print("#配置尝试次数：");
    Serial.println(tryNum);    // 输出当前尝试次数
    tryNum++;                  // 递增尝试计数器
  }

  type = ps2x.readType();      // 读取并存储手柄类型
}

/*======================= 主循环函数 ========================*/
void loop() {
  if (type == 1) {             // 仅当确认是标准手柄时执行
    ps2x.read_gamepad(false, vibrate);  // 读取手柄数据（不振动）

    /*-------------- 右摇杆舵机控制部分 --------------*/

    int rightStickX = ps2x.Analog(PSS_RX);  // 读取右摇杆X轴原始值（0-255）
    int rightStickY = ps2x.Analog(PSS_RY);  // 读取右摇杆Y轴（当前未使用）
  
    // 摇杆值转换：将0-254范围转换为±20角度增量（排除中间值127）
    a1 = map(rightStickX, 0, 254, -20, 20);
    servoAngle += a1;                   // 应用角度增量
    // 摇杆归中处理：当检测到中间值时强制角度为90度
    if(rightStickX == 127){ servoAngle = 90; }
    servoAngle = constrain(servoAngle, 0, 180);  // 限制舵机角度范围
    myServo.write(servoAngle);          // 写入实际角度值

    /*-------------- 左摇杆电机控制部分 --------------*/

    int leftStickY = ps2x.Analog(PSS_LY);  // 读取左摇杆Y轴原始值
    
    // 前推信号处理（0-127区间）
    int actualInput = constrain(leftStickY, 0, 127);  // 限制有效输入范围
    v = map(actualInput, 0, 127, 255, 0);   // 倒转映射：前推到底对应255速度

    // 后拉信号处理（128-255区间）
    int reverseInput = constrain(leftStickY - 128, 0, 127);  // 提取后拉分量
    int reverseSpeed = map(reverseInput, 0, 127, 0, 255);    // 转换为正向速度

    /*----------- 电机方向控制逻辑 -----------*/

    // 前推优先判断：速度超过12时前进（消除中间死区）
    if(v > 12) { 
      digitalWrite(IN1, HIGH);   // 设置方向为正向
      digitalWrite(IN2, LOW);
      analogWrite(ENA, v);       // 输出PWM速度
    }
    // 后拉判断：当前未前进且后拉有效时触发
    else if(reverseSpeed > 12) {
      digitalWrite(IN1, LOW);    // 设置方向为反向
      digitalWrite(IN2, HIGH);
      analogWrite(ENA, reverseSpeed); 
    }
    // 中立位停止
    else {  
      analogWrite(ENA, 0);       // 关闭电机输出
    }

    /*----------- 串口调试信息输出 -----------*/
    
    Serial.print(" |电机速度: ");      // 前推速度（新增）
    Serial.print(v);
    Serial.print(" |反速:");           // 后拉速度（新增）
    Serial.print(reverseSpeed); 
    Serial.print(" |右X: ");           // 右摇杆X原始值
    Serial.print(rightStickX);
    Serial.print(" |左Y原始: ");       // 左摇杆原始值
    Serial.print(leftStickY);
    Serial.print(" |处理左Y: ");       // 前推处理后的值
    Serial.print(actualInput);
    Serial.print(" |舵机: ");          // 当前舵机角度
    Serial.println(servoAngle);
  }
  delay(50);  // 主循环延时（约20Hz刷新率）
}

